#!/usr/bin/python3
# coding=gbk

import rospy
import cv2
import numpy as np
 
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage
class image_converter:
    def __init__(self):    
        self.image_sub_f = rospy.Subscriber("/usb_cam_f/image_raw", Image, self.callbackf)
        self.image_pub_f = rospy.Publisher("/camera_f/fpv", Image, queue_size=1)
        self.image_sub_b = rospy.Subscriber("/usb_cam_b/image_raw", Image, self.callbackb)
        self.image_pub_b = rospy.Publisher("/camera_b/fpv", Image, queue_size=1)
        self.bridge = CvBridge()

    def callbackf(self,data):

        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            exit()#print e


        (rows,cols,channels) = cv_image.shape
        cv_image=cv2.rotate(cv_image,cv2.ROTATE_90_CLOCKWISE) 
        cv_image=cv2.rotate(cv_image,cv2.ROTATE_180) 
        crop = cv2.resize(cv_image,(320,240))
        gray2=cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY)
 
        try:
            self.image_pub_f.publish(self.bridge.cv2_to_imgmsg(gray2 , "mono8"))
        except CvBridgeError as e:
            exit()#print e

    def callbackb(self,data):

        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            exit()#print e


        (rows,cols,channels) = cv_image.shape
        cv_image=cv2.rotate(cv_image,cv2.ROTATE_90_COUNTERCLOCKWISE) 
        cv_image=cv2.rotate(cv_image,cv2.ROTATE_180) 
        crop = cv2.resize(cv_image,(320,240))
        gray2=cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY)
 
        try:
            self.image_pub_b.publish(self.bridge.cv2_to_imgmsg(gray2 , "mono8"))
        except CvBridgeError as e:
            exit()#print e
if __name__ == '__main__':
    try:
        rospy.init_node("cv_bridge_test")
        rospy.loginfo("Tinymal::starting FPV camera node")
        image_converter()
        rospy.spin()
    except KeyboardInterrupt:
        print("Tinymal::shutting down FPV camera node.")
        cv2.destroyAllWindows()
